module Toolbox
    
contains

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!!                                       !!!!!
!!!!!       Matrix Inverse Computation      !!!!!
!!!!!                                       !!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! inv(A) computes the inverse of matrix A
!
! Input:
!   A = n x n matrix
!
! Output:
!   Ainv = n x n matrix, inverse of A

function inv_FET(A) result(Ainv)

  real(8), dimension(:,:), intent(in)     :: A  
  real(8), dimension(size(A,1),size(A,2)) :: Ainv

  real(8), dimension(size(A,1)) :: work  ! work array for MKL
  integer, dimension(size(A,1)) :: ipiv  ! pivot indices
  integer :: n, info    

  ! Store A in Ainv to prevent it from being overwritten by MKL
  Ainv = A
  n    = size(A,1)

  ! DGETRF computes an LU factorization of a general M-by-N matrix A
  ! using partial pivoting with row interchanges.
  call DGETRF(n, n, Ainv, n, ipiv, info)
  ! call SGETRF(n, n, Ainv, n, ipiv, info)

  if (info /= 0) then
     stop 'Matrix is numerically singular!'
  end if

  ! DGETRI computes the inverse of a matrix using the LU factorization
  ! computed by DGETRF.
  call DGETRI(n, Ainv, n, ipiv, work, n, info)
  ! call SGETRI(n, Ainv, n, ipiv, work, n, info)

  if (info /= 0) then
     stop 'Matrix inversion failed!'
  end if
  
end function inv_FET


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!!                                              !!!!!
!!!!!              matrix multiplication           !!!!!
!!!!!                                              !!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! matmul_FET(A,B,C) computes the multiplication of A times B and stores it in C
! 
! Input:
!   A = rA x cA matrix (real)
!   B = rB x cB matrix (real)
!   need cA = rB otherwise an error occurs
!
! Output:
!   C = rA x cB   matrix (real)

subroutine matmul_FET(A,B,C)

    implicit none
    real(8), intent(in)  :: A(:,:), B(:,:)
    real(8), intent(out) :: C(size(A,1),size(B,2))
    
    integer :: rA, cA, rB, cB
    ! integer :: icA, icB, ic, cK_l, cK_r    
    
    
    rA = size(A,1)
    cA = size(A,2)
    rB = size(B,1)
    cB = size(B,2)
    
    if (cA .NE. rB ) then
        stop 'ERROR: columns(A) \= rows(B), cannot multiply AxB!'        
    end if
    
    call dgemm('N', 'N', rA, cB, cA, 1.0D0, A, rA, B, rB, 0.0D0, C, rA)

end subroutine matmul_FET


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!!                                              !!!!!
!!!!!       matrix x vector multiplication         !!!!!
!!!!!                                              !!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! matvec_FET(A,V,Y) computes the multiplication of matrix A times vector V and stores it in vector Y
! 
! Input:
!   A = rA x cA matrix (real)
!   V = rV x 1  vector (real)
!   need cA = rV otherwise an error occurs
!
! Output:
!   Y = rA x 1   vector (real)

subroutine matvec_FET(A,V,Y)

    implicit none
    real(8), intent(in)  :: A(:,:), V(:)
    real(8), intent(out) :: Y(size(A,1))
    
    integer :: rA, cA, rV
    
    rA = size(A,1)
    cA = size(A,2)
    rV = size(V)
    
    if (cA .NE. rV ) then
        stop 'ERROR: columns(A) \= rows(V), cannot multiply AxV!'        
    end if
    
    call dgemv('N', rA, cA, 1.0D0, A, rA, V, 1, 0.0D0, Y, 1)
    
    ! call dgemv(trans, m, n, alpha, a, lda, x, incx, beta, y, incy)
    

end subroutine matvec_FET


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!!                                   !!!!!    
!!!!!     Search points in a vector     !!!!!   
!!!!!                                   !!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! vsearchprm(y,v,vprm) returns an integer with the two closest points in v to y
!
! Input: 
!   y    = Ny x 1 vector
!   v    = Nv x 1 vector
!   vprm = real - curvature parameter in v
!
! Output:
!   ind = Ny x 1 vector
! 
! Routine assumes v is constructed as: 
!			v = vmin + (vmax-vmin)*(x**1/vprm)
! with vmin = v(1), vmax = v(Nv), where x is an Nv by 1 vector uniformly distributed between 0 and 1.
! vprm is the curvature: vprm = 0 for L-shaped, vprm = 1 for linear
!
!
! Note:
! if y(i) <= v(1) : ind(i) = 1
! if y(i) >= v(p) : ind(i) = p-1
! else            : v(ind(i)) <= y(i) < v(ind(i)+1)

function vsearchprm_FET(y,v,vprm) result(ind)

	implicit none
	real(8), intent(in)           :: y(:), v(:), vprm
	integer, dimension(size(y,1)) :: ind
	
	real(8), dimension(size(y,1)) :: yhat, nhat
	real(8) :: Dx, vmin, vmax	
	integer :: Ny, Nv	
	
	Ny = size(y,1); Nv = size(v,1);
	Dx = 1.0D0/dble(Nv-1)
	
	vmin = v(1); vmax = v(Nv)
	
	yhat = ((y-vmin)/(vmax-vmin))**vprm
	nhat = 1.0D0 + (yhat/Dx)
	
	ind = floor(nhat)
	
	where (y >= vmax) ind = Nv-1
	where (y <= vmin) ind = 1

end function vsearchprm_FET


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!!!!                                   !!!!!    
!!!!!        Equally spaced grid        !!!!!   
!!!!!                                   !!!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

function LINSPACE_FET(xlb,xub,Nx)  result(xout)

	implicit none
	
	real(8), intent(in) :: xlb, xub
	integer, intent(in) :: Nx

	real(8)             :: xout(Nx)
	
	real(8) :: dx
	integer :: ix
	
	dx = (xub-xlb)/(dble(Nx)-1.0D0)
	
	xout = [(xlb + dx*dble(ix-1), ix = 1, Nx)]   	
	

end function LINSPACE_FET

    
end module Toolbox

